#!/usr/bin/env python
import roslib
roslib.load_manifest('ipm_to_laserscan')
import sys
import rospy
import cv
import time
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class GrabCutNode:

    def __init__(self):
        rospy.init_node('grabcut_node')

        """ Create the cv_bridge object """
        self.bridge = CvBridge()

        """ Subscribe to the raw camera image topic """
        self.image_in_topic = rospy.get_param('~image_in_topic', "/camera/image_raw")
        self.image_out_topic = rospy.get_param('~image_out_topic', "camera/image_grabcut")

        self.point = None
        self.flags = cv.CV_FLOODFILL_FIXED_RANGE | cv.CV_FLOODFILL_MASK_ONLY | (255 << 8)
        self.debug = True

        self.image_sub = rospy.Subscriber(self.image_in_topic, Image, self.handle_image)
        self.image_pub = rospy.Publisher(self.image_out_topic,Image)

        cv.NamedWindow("Mean", 0)
    def handle_image(self, data):
        try:
            """ Convert the raw image to OpenCV format """
            src = self.bridge.imgmsg_to_cv(data, "bgr8")
            self.process_image(src)
        except CvBridgeError, e:
            print e
            return

    def process_image(self, img):
            self.orig_size = (width, height) = cv.GetSize(img)

            out = cv.CreateImage(self.orig_size, cv.IPL_DEPTH_8U, 3)
            
            rospy.loginfo(str(cv.GrabCut))
            cv.Circle(img, (width/2, height), 15,(255, 255, 255), -1)
            cv.ShowImage("Mean", img)
            cv.WaitKey(3)

            image_message = self.bridge.cv_to_imgmsg(out, encoding="passthrough")
            self.image_pub.publish(image_message)
            
def main(args):
      vn = GrabCutNode()
      try:
        rospy.spin()
      except KeyboardInterrupt:
        print "Shutting down hsv node."
      cv.DestroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

